/* ========================================
 *
 * Copyright YOUR COMPANY, THE YEAR
 * All Rights Reserved
 * UNPUBLISHED, LICENSED SOFTWARE.
 *
 * CONFIDENTIAL AND PROPRIETARY INFORMATION
 * WHICH IS THE PROPERTY OF your company.
 *
 * ========================================
*/

/* [] END OF FILE */


#include <stdlib.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"

/* Demo program include files. */
#include "project.h"
#include "main.h"
#include "tstate.h"

typedef struct _single_brushing_data
{
    uint32 time;
    uint32 model;
    uint16 pos[16];        

}single_brushing_data;

int current_time = 0;
float base_pitch = 0;
float base_orientation = 0;
extern Attitude_Data attitude_atri;
single_brushing_data one_brush;


void compute_position(void)
{
    float roll,pitch,compass;
    

    roll = attitude_atri.ned[0]*0.1;
    pitch = attitude_atri.ned[1]*0.1;
    compass = attitude_atri.ned[2]*0.1;
    
    if(current_time < 300)
    {
        if(current_time < 3)
        {
            base_orientation = compass + 90;
            base_orientation = base_orientation > 360 ? base_orientation - 360 : base_orientation;
            base_pitch = roll;
            one_brush.pos[12]++;
        }
        else
        {
            
        }
        
        
        
//        if(position != 0xfe)
//            one_brush.pos[position] += 1;
//        cmd_async_feedback_F4(position);
        current_time += 1;
    }
}

portTASK_FUNCTION( vTaskStats, pvParameters )
{  
    char task_state[300];
    extern char power_state;
    extern char state_change;
    extern TaskHandle_t Rd_task;
    extern TaskHandle_t Ff_task;
    extern TaskHandle_t Mc_task;
   
    ( void ) pvParameters;
    
    vTaskSuspend(Rd_task);
    vTaskSuspend(Ff_task);
    vTaskSuspend(Mc_task);
                
    for(;;)
    {
        vTaskDelay( 1000 );

		/* Delay for half the flash period then turn the LED off. */

        
        memset(task_state,0,sizeof(task_state));
        vTaskGetRunTimeStats(task_state);
        UART_DEB_1_PutString(task_state);
 
        if(state_change)
        {
            UART_DEB_1_PutString("key state has been changed!\n");
            state_change = 0;
            if(!power_state)
            {
                UART_DEB_1_PutString("power_state off!\n");
//                FXOS8700_Close();
//                FXAS2100X_Close();
                vTaskSuspend(Rd_task);
                vTaskSuspend(Ff_task);
                vTaskSuspend(Mc_task);
                Pin_LED_1_Write(LED_OFF);
            }
            else
            {
                UART_DEB_1_PutString("power_state on!\n");
//                FXOS8700_Open();
//                FXAS2100X_Open();
                Fusion_task_init();
                vTaskResume(Rd_task);
                vTaskResume(Ff_task);
                vTaskResume(Mc_task);
                Pin_LED_1_Write(LED_ON);
                current_time = 0;
            }
        }
        
        if(power_state)        
            compute_position();
    }
}  


void vStartTstateTasks( UBaseType_t uxPriority )
{
	/* Create the three tasks. */
		/* Spawn the task. */
	xTaskCreate( vTaskStats, "TaskState", 300, NULL, uxPriority, ( TaskHandle_t * ) NULL );
}